#!/usr/bin/env python
import roslib; roslib.load_manifest('smc_04b')
import rospy
import serial
from geometry_msgs.msg import Twist
def callback(vels):
	rospy.loginfo(rospy.get_name()+" motor command received: %s", vels)

def vels_to_motor_cmd(vels):
	ang_vel_left.x = vels.linear.x - b * vels.angular.x
	ang_vel_left.y = vels.linear.y - b * vels.angular.y

	ang_vel_right.x = vels.linear.x + b * vels.angular.x
	ang_vel_right.y = vels.linear.y + b * vels.angular.y
		# Convert desired angular and linear velocities of the whole robot to linear and angular velocities of each wheel.

		# Choose appropriate wheel command based on desired velocities of each wheel

		# Compose command
		# Send command

		
	
def cmd_to_port(vels):
	cmd = vels_to_motor_cmd(vels)
		# send vels to be converted to motor commands
	s = serial.Serial('/dev/ttyUSB0', 9600)
		# Open serial connection to each wheel and send
	s.write(chr(cmd.data))
	callback(cmd)	

def driver():
	rospy.init_node('twist_to_mot_driver', anonymous=True)
		#Initialize the twist_to_mot_driver node, 
	rospy.Subscriber("cmd_vel", Twist, cmd_to_port)
		#Begin recieving Twist messages from the cmd_vel topic, send these messages to cmd_to_port. cmd_to_port receives them as vels
	rospy.spin() 
		# keep doing this

if __name__ == '__main__':
	driver()
		#Begin the driver
